package edu.umd.essic.maskbuilder;

import java.lang.Math;

public class GG_Utilities {
    
 public static double ESSICfunc(double coord, double[] abc, float[] alpha, float[] beta, float[] gamma, int N){
    double PSI;
    double sum = 0.0;
    for (int k=0; k<N; k++){
            sum = sum + alpha[k]*Math.atan((coord-gamma[k])/beta[k]);
        }
    return PSI = (abc[0]+abc[1]*(coord+sum)+abc[2]*Math.pow(coord,2));
}

/*private double ESSICmodif(float coord1, float coord2, double a, double b, double c, 
                          float[] alpha, float[] beta, float[] gamma1, float[] gamma2, int N){
    double PSI;
    double sum = 0.0;
    for (int k=0; k<N; k++){
            sum = sum + alpha[k]*Math.atan((coord1-gamma1[k])*(coord2-gamma2[k])/beta[k]);
        }
    return PSI = (a+b*(coord1+sum)+c*Math.pow(coord1,2));
}*/

public static double EXPfunc(double coord1, float[] center1, double coord2, float[] center2, 
                       float[] alpha, float[] sigma1, float[] sigma2, int N){
    double PSI;
    double sum = 0.0;
    for (int k=0; k<N; k++)
            sum = sum + alpha[k]*(coord1-center1[k])*Math.exp(-(Math.pow((coord1-center1[k])/sigma1[k],2))-(Math.pow((coord2-center2[k])/sigma2[k],2)));
    return PSI = (coord1+sum);
}

public static double EXPfunc_expand(double coord1, float[] center1, double coord2, float[] center2, 
                       float alpha, float sigma1, float sigma2, int N){
    double PSI;
    double sum = 0.0;
    for (int k=0; k<N; k++)
            sum = sum + (coord1-center1[k])*Math.exp(-(Math.pow((coord1-center1[k])/sigma1,2))-(Math.pow((coord2-center2[k])/sigma2,2)));
    return PSI = (coord1-sum*alpha);
}

public static double[] getESSIC_ABC(double xmin, double xmax, float[] alpha, float[] beta, float[] gamma, int N, boolean flag_equator){
    double[] abc = { 0.0,1.0,0.0 }; 
    double A,B,C;
    double fmin = ESSICfunc(xmin,abc,alpha,beta,gamma,N);
    double fmax = ESSICfunc(xmax,abc,alpha,beta,gamma,N);
    if ((xmin*xmax>=0)||(!flag_equator)){
        A = (xmin*fmax-fmin*xmax)/(fmax-fmin);
        B = (xmax-xmin)/(fmax-fmin);
        C = 0.0;
    } else{// Common case, if xmin can be negative and 0 corresponds to equator
        double x1 = xmin*xmin;
        double x2 = xmax*xmax;
        double f0   = ESSICfunc(0.f,abc,alpha,beta,gamma,N);
        B = (xmin*x2-xmax*x1)/(x2*(fmin-f0)-x1*(fmax-f0));
        A = -B*f0;
        C = (xmin-B*(fmin-f0))/x1;
    }
    abc[0] = A;
    abc[1] = B;
    abc[2] = C;
    return abc; 
}

/*private void getESSICmodif_ABC(float xmin, float xmax, float ycur,  
                               float[] alpha, float[] beta, float[] gamma1, float[] gamma2, int N){
    double fmin = ESSICmodif(xmin,ycur,0.0,1.0,0.0,alpha,beta,gamma1,gamma2,N);
    double fmax = ESSICmodif(xmax,ycur,0.0,1.0,0.0,alpha,beta,gamma1,gamma2,N);
    if (xmin*xmax>=0){
        A = (xmin*fmax-fmin*xmax)/(fmax-fmin);
        B = (xmax-xmin)/(fmax-fmin);
        C = 0.0;
    } else{// Common case, if xmin can be negative and 0 corresponds to equator
        float x1 = xmin*xmin;
        float x2 = xmax*xmax;
        double f0   = ESSICmodif(0.f,ycur,0.0,1.0,0.0,alpha,beta,gamma1,gamma2,N);
        B = (xmin*x2-xmax*x1)/(x2*(fmin-f0)-x1*(fmax-f0));
        A = -B*f0;
        C = (xmin-B*(fmin-f0))/x1;
    }
}*/

public static double DiffESSIC(double coord, double[] abc, float[] alpha, float[] beta, float[] gamma,int N){
    float sum = 0;
    for (int k=0; k<N; k++) sum = (float) (sum + alpha[k]*beta[k]/(beta[k]*beta[k]+Math.pow(coord-gamma[k],2)));
    return (float)(abc[1]*(1+sum)+2*abc[2]*coord);
}

public static double Diff2ESSIC(double coord, double[] abc, float[] alpha, float[] beta, float[] gamma, int N){
    float sum = 0;
    for (int k=0; k<N; k++){ 
        sum = (float) (sum - 2*alpha[k]*(coord-gamma[k])*beta[k]/Math.pow((beta[k]*beta[k]+Math.pow(coord-gamma[k],2)),2));}
    return (float)(abc[1]*sum+2*abc[2]);
}

public static double DiffExp(double coord1, float[] cen_1, double coord2, float[] cen_2, float[] skew, float[] dim1, float[] dim2, int N){
    float sum = 0;
    for (int k=0; k<N; k++){
        sum = (float)(sum + skew[k]*Math.exp(-Math.pow(coord1-cen_1[k],2)/(dim1[k]*dim1[k])-Math.pow((coord2-cen_2[k])/dim2[k],2))-
                      2*skew[k]*Math.pow(coord1-cen_1[k],2)*Math.exp(-Math.pow(coord1-cen_1[k],2)/(dim1[k]*dim1[k])-Math.pow((coord2-cen_2[k])/dim2[k],2))/(dim1[k]*dim1[k])); }
    return (1+sum);
}

public static double Diff2Exp(double coord1, float[] cen_1, double coord2, float[] cen_2, float[] skew, float[] dim1, float[] dim2, int N){
    float sum = 0;
    for (int k=0; k<N; k++){
        sum = (float) (sum - skew[k]* Math.exp(-Math.pow(coord1-cen_1[k],2)/(dim1[k]*dim1[k])-Math.pow((coord2-cen_2[k])/dim2[k],2))*
                       (6*(coord1-cen_1[k])/(dim1[k]*dim1[k])+4*Math.pow(coord1-cen_1[k],3)/Math.pow(dim1[k],4)));}
    return sum;
}

public static double Newton(double xmin, double xmax, double xreg, double[] abc, float[] alpha, float[] beta, float[] gamma, int N){
// to solve the equation f(xstr) = xreg - psi(xstr) = 0
// by iterations xnext = xcur + delx or xnext = xcur-f(xcur)/f'(xcur).
     double xcur;
     double xnext;
     double f,fp,psi1,psi2;
     int iter;
     int max_it = 1000;
     int numstart= 100; 
     int k = 1;
     double epsilon = 1e-7f;
     double delx = (xmax-xmin)/(numstart-1);
     psi1 = ESSICfunc(xmin,abc,alpha,beta,gamma,N);

     // first find the first approximation of solution
     // such that psi1<=xreg<=psi2 (or psi2<=xreg<=psi1 in case of negative delx) 
     // and then start iterations
     if (delx >= 0){
     do{
     psi2 = ESSICfunc(xmin+k*delx,abc,alpha,beta,gamma,N);
     if (psi2 >= xreg) break;
     if (k <= numstart-1){
         k++;
         psi1 = psi2;
     }
     }while (psi2 < xreg);}
     else { 
     do{
     psi2 = ESSICfunc(xmin+k*delx,abc,alpha,beta,gamma,N);
     if (psi2 <= xreg) break;
     if (k <= numstart-1){
         k++;
         psi1 = psi2;
     }
     }while (psi2 > xreg);}

     xnext = (xmin+(k-1)*delx+(xreg-psi1)*delx/(psi2-psi1));
     iter = 0;
     // iteration loop:
     do{
         xcur = xnext;
         f = ESSICfunc(xcur,abc,alpha,beta,gamma,N)-xreg;
         fp= DiffESSIC(xcur,abc,alpha,beta,gamma,N);
         xnext = xcur-f/fp;
         iter++;
         if (iter >=max_it) break;
     } while (((Math.abs((xnext-xcur)/xcur)> epsilon) && (xcur != 0.0)) ||((Math.abs(xnext) > epsilon) && (xcur == 0.0))); 
     return xcur;
}

public static double Newton(double xmin, double xmax, double xreg, double yreg, 
                     float[] center1, float[] center2, float[] skew, float[] dim1, float[] dim2, int N){
// to solve the equation f(xstr) = xreg - psi(xstr) = 0
// by iterations xnext = xcur + delx or xnext = xcur-f(xcur)/f'(xcur).
     double xcur;
     double xnext;
     double f,fp,psi1,psi2;
     int iter;
     int max_it = 1000;
     int numstart= 100; 
     int k = 1;
     double epsilon = 1e-7f;
     double delx = (xmax-xmin)/(numstart-1);
     psi1 = EXPfunc(xmin,center1,yreg,center2,skew,dim1,dim2,N);

     // first find the first approximation of solution
     // such that psi1<=xreg<=psi2 (or psi2<=xreg<=psi1 in case of negative delx) 
     // and then start iterations
     if (delx >= 0){
     do{
     psi2 = EXPfunc(xmin+k*delx,center1,yreg,center2,skew,dim1,dim2,N);
     if (psi2 >= xreg) break;
     if (k <= numstart-1){
         k++;
         psi1 = psi2;
     }
     }while (psi2 < xreg);}
     else { 
     do{
     psi2 = EXPfunc(xmin+k*delx,center1,yreg,center2,skew,dim1,dim2,N);
     if (psi2 <= xreg) break;
     if (k <= numstart-1){
         k++;
         psi1 = psi2;
     }
     }while (psi2 > xreg);}

     xnext = (xmin+(k-1)*delx+(xreg-psi1)*delx/(psi2-psi1));
     iter = 0;
     // iteration loop:
     do{
         xcur = (float)xnext;
         f = EXPfunc(xcur,center1,yreg,center2,skew,dim1,dim2,N)-xreg;
         fp= DiffExp(xcur,center1,yreg,center2,skew,dim1,dim2,N);
         xnext = (float) (xcur-f/fp);
         iter++;
         if (iter >=max_it) break;
     } while (((Math.abs((xnext-xcur)/xcur)> epsilon) && (xcur != 0.0)) ||((Math.abs(xnext) > epsilon) && (xcur == 0.0))); 
     return xcur;
}

public static double Newton1(double xmin, double xmax, double xreg, double ycur, double[] abc,
                     float[] alpha, float[] beta, float[] gamma, int N1, float[] skew, float[] dim1, float[] dim2, float[] center1, float[] center2, int N2){
// to solve the equation f(xstr) = xreg - psi(xstr) = 0
// by iterations xnext = xcur + delx or xnext = xcur-f(xcur)/f'(xcur).
     double xcur;
     double xnext;
     double f,fp,psi1,psi2;
     int iter;
     int max_it = 1000;
     int numstart= 100; 
     int k = 1;
     double epsilon = 1e-7f;
     double delx = (xmax-xmin)/(numstart-1);
     psi1 = ESSICfunc(EXPfunc(xmin,center1,ycur,center2,skew,dim1,dim2,N2),abc,alpha,beta,gamma,N1);

     // first find the first approximation of solution
     // such that psi1<=xreg<=psi2 (or psi2<=xreg<=psi1 in case of negative delx) 
     // and then start iterations
     if (delx >= 0){
     do{
     psi2 = ESSICfunc(EXPfunc(xmin+k*delx,center1,ycur,center2,skew,dim1,dim2,N2),abc,alpha,beta,gamma,N1);
     if (psi2 >= xreg) break;
     if (k <= numstart-1){
         k++;
         psi1 = psi2;
     }
     }while (psi2 < xreg);}
     else { 
     do{
     psi2 = ESSICfunc(EXPfunc(xmin+k*delx,center1,ycur,center2,skew,dim1,dim2,N2),abc,alpha,beta,gamma,N1);
     if (psi2 <= xreg) break;
     if (k <= numstart-1){
         k++;
         psi1 = psi2;
     }
     }while (psi2 > xreg);}

     xnext = (xmin+(k-1)*delx+(xreg-psi1)*delx/(psi2-psi1));
     iter = 0;
     // iteration loop:
     do{
         xcur = xnext;
         f = ESSICfunc(EXPfunc(xcur,center1,ycur,center2,skew,dim1,dim2,N2),abc,alpha,beta,gamma,N1)-xreg;
         fp= DiffESSIC(EXPfunc(xcur,center1,ycur,center2,skew,dim1,dim2,N2),abc,alpha,beta,gamma,N1)*DiffExp(xcur,center1,ycur,center2,skew,dim1,dim2,N2);
         xnext = xcur-f/fp;
         iter++;
         if (iter >=max_it) break;
     } while (((Math.abs((xnext-xcur)/xcur)> epsilon) && (xcur != 0.0)) ||((Math.abs(xnext) > epsilon) && (xcur == 0.0))); 
     return xcur;
}

public static double Newton(double xmin, double xmax, double xreg, double ycur, double[] abc,
                     float[] alpha, float[] beta, float[] gamma, int N1, float[] skew, float[] dim1, float[] dim2, float[] center1, float[] center2, int N2){
// to solve the equation f(xstr) = xreg - psi(xstr) = 0
// by iterations xnext = xcur + delx or xnext = xcur-f(xcur)/f'(xcur).
     double xcur;
     double xnext;
     double f,fp,psi1,psi2;
     int iter;
     int max_it = 1000;
     int numstart= 100; 
     int k = 1;
     double epsilon = 1e-7f;
     double delx = (xmax-xmin)/(numstart-1);
     psi1 = EXPfunc(ESSICfunc(xmin,abc,alpha,beta,gamma,N1),center1,ycur,center2,skew,dim1,dim2,N2);

     // first find the first approximation of solution
     // such that psi1<=xreg<=psi2 (or psi2<=xreg<=psi1 in case of negative delx) 
     // and then start iterations
     if (delx >= 0){
     do{
     psi2 = EXPfunc(ESSICfunc(xmin+k*delx,abc,alpha,beta,gamma,N1),center1,ycur,center2,skew,dim1,dim2,N2);
     if (psi2 >= xreg) break;
     if (k <= numstart-1){
         k++;
         psi1 = psi2;
     }
     }while (psi2 < xreg);}
     else { 
     do{
     psi2 = EXPfunc(ESSICfunc(xmin+k*delx,abc,alpha,beta,gamma,N1),center1,ycur,center2,skew,dim1,dim2,N2);
     if (psi2 <= xreg) break;
     if (k <= numstart-1){
         k++;
         psi1 = psi2;
     }
     }while (psi2 > xreg);}

     xnext = (xmin+(k-1)*delx+(xreg-psi1)*delx/(psi2-psi1));
     iter = 0;
     // iteration loop:
     do{
         xcur = xnext;
         f = EXPfunc(ESSICfunc(xcur,abc,alpha,beta,gamma,N1),center1,ycur,center2,skew,dim1,dim2,N2)-xreg;
         fp= DiffExp(ESSICfunc(xcur,abc,alpha,beta,gamma,N1),center1,ycur,center2,skew,dim1,dim2,N2)*DiffESSIC(xcur,abc,alpha,beta,gamma,N1);
         xnext = xcur-f/fp;
         iter++;
         if (iter >=max_it) break;
     } while (((Math.abs((xnext-xcur)/xcur)> epsilon) && (xcur != 0.0)) ||((Math.abs(xnext) > epsilon) && (xcur == 0.0))); 
     return xcur;
}

}
